|
系列文章目录
文章目录
系列文章目录1. VINS-Mono框架介绍2. VINS前端逻辑2.1 图解前端2.2 代码中的当前帧2.3 图解光流
3. 前端代码详解3.1 主函数3.2 回调函数3.3 readImage函数3.4 rejectWithF函数
4. 特征点管理代码详解5. processImage函数6. 图解VINS特征点管理策略
1. VINS-Mono框架介绍
 每一部分的功能： 
2. VINS前端逻辑
2.1 图解前端
2.2 代码中的当前帧
2.3 图解光流
已知：上一帧图像以及观测到的特征点得到当前帧对当前帧进行直方图均衡化调用OpenCV函数在当前帧上利用光流法跟踪上一帧的特征点剔除：跟踪失败的特征点、在图像边缘的特征点、外点剔除后，为了保证特征点数量不太少，需要提取新的特征点首先在跟踪成功的特征点附近设置Mask，Mask中不会提取特征点，保证特征点能够均匀分布调用OpenCV函数提取新的特征点对特征点进行归一化和去畸变处理
3. 前端代码详解
3.1 主函数
int main(int argc, char **argv)
{
ros::init(argc, argv, "feature_tracker"); // ros节点初始化
ros::NodeHandle n("~"); // 声明一个句柄，～代表这个节点的命名空间
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); // 设置ros log级别
readParameters(n); // 读取配置文件
for (int i = 0; i
trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
if(!trackerData[i].fisheye_mask.data)
{
ROS_INFO("load mask fail");
ROS_BREAK();
}
else
ROS_INFO("load mask success");
}
}
// 这个向roscore注册订阅这个topic，收到一次message就执行一次回调函数
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
// 注册一些publisher
pub_img = n.advertise("feature", 1000); // 实际发出去的是 /feature_tracker/feature
pub_match = n.advertise("feature_img",1000);
pub_restart = n.advertise("restart",1000);
/*
if (SHOW_TRACK)
cv::namedWindow("vis", cv::WINDOW_NORMAL);
*/
ros::spin(); // spin代表这个节点开始循环查询topic是否接收
return 0;
}
ROS节点初始化，声明ROS句柄，设置log级别读取配置文件读取每个相机的内参和畸变参数判断是否加入鱼眼向roscore注册订阅这个topic，收到一次message就执行一次回调函数发布跟踪到的特征点、带有特征点的图像、重启信息
3.2 回调函数
if(first_image_flag) // 对第一帧图像的基本操作
{
first_image_flag = false;
first_image_time = img_msg->header.stamp.toSec();
last_image_time = img_msg->header.stamp.toSec();
return;
}
// detect unstable camera stream
// 检查时间戳是否正常，这里认为超过一秒或者错乱就异常
// 图像时间差太多光流追踪就会失败，这里没有描述子匹配，因此对时间戳要求就高
if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec()
PUB_THIS_FRAME = true;
// reset the frequency control
// 这段时间的频率和预设频率十分接近，就认为这段时间很棒，重启一下，避免delta t太大
if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ)
sensor_msgs::Image img;
img.header = img_msg->header;
img.height = img_msg->height;
img.width = img_msg->width;
img.is_bigendian = img_msg->is_bigendian;
img.step = img_msg->step;
img.data = img_msg->data;
img.encoding = "mono8";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
}
else
ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
判断是否是第一帧图像，如果是，记录第一帧图像的时间检查时间戳是否正常，如果间隔大于1秒或当前时间戳小于上一时间戳，则返回重启信息控制发给后端的频率，保证频率在10Hz上下进行图像格式转换，将ROS图像转换为CV图像
cv::Mat show_img = ptr->image;
TicToc t_r;
for (int i = 0; i
if (EQUALIZE)
{
cv::Ptr clahe = cv::createCLAHE();
clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
}
else
trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
}
#if SHOW_UNDISTORTION
trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
}
对于不同的相机进行处理 对于第一个相机，使用readImage进行处理 对于后续的相机，只进行直方图均衡化处理
for (unsigned int i = 0;; i++)
{
bool completed = false;
for (int j = 0; j
auto &un_pts = trackerData[i].cur_un_pts; // 去畸变的归一化相机坐标系
auto &cur_pts = trackerData[i].cur_pts; // 像素坐标
auto &ids = trackerData[i].ids; // id
auto &pts_velocity = trackerData[i].pts_velocity; // 归一化坐标下的速度
for (unsigned int j = 0; j
int p_id = ids[j];
hash_ids[i].insert(p_id); // 这个并没有用到
geometry_msgs::Point32 p;
p.x = un_pts[j].x;
p.y = un_pts[j].y;
p.z = 1;
// 利用这个ros消息的格式进行信息存储
feature_points->points.push_back(p);
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
u_of_point.values.push_back(cur_pts[j].x);
v_of_point.values.push_back(cur_pts[j].y);
velocity_x_of_point.values.push_back(pts_velocity[j].x);
velocity_y_of_point.values.push_back(pts_velocity[j].y);
}
}
}
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);
ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
// skip the first image; since no optical speed on frist image
if (!init_pub)
{
init_pub = 1;
}
else
pub_img.publish(feature_points); // 前端得到的信息通过这个publisher发布出去
将特征点的归一化坐标、ID、像素坐标、速度发布给后端第一帧不发布
3.3 readImage函数
cv::Mat img;
TicToc t_r;
cur_time = _cur_time;
if (EQUALIZE)
{
// 图像太暗或者太亮，提特征点比较难，所以均衡化一下
// ! opencv 函数看一下
cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
TicToc t_c;
clahe->apply(_img, img);
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else
img = _img;
// 这里forw表示当前，cur表示上一帧
if (forw_img.empty()) // 第一次输入图像，prev_img这个没用
{
prev_img = cur_img = forw_img = img;
}
else
{
forw_img = img;
}
forw_pts.clear();
if (cur_pts.size() > 0) // 上一帧有特征点，就可以进行光流追踪了
{
TicToc t_o;
vector status;
vector err;
// 调用opencv函数进行光流追踪
// Step 1 通过opencv光流追踪给的状态位剔除outlier
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
for (int i = 0; i
if(mask.empty())
cout
ROS_DEBUG("FM ransac begins");
TicToc t_f;
vector un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
for (unsigned int i = 0; i
int v = img_msg->channels[0].values[i] + 0.5;
int feature_id = v / NUM_OF_CAM;
int camera_id = v % NUM_OF_CAM;
double x = img_msg->points[i].x; // 去畸变后归一滑像素坐标
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
double p_u = img_msg->channels[1].values[i]; // 特征点像素坐标
double p_v = img_msg->channels[2].values[i];
double velocity_x = img_msg->channels[3].values[i]; // 特征点速度
double velocity_y = img_msg->channels[4].values[i];
ROS_ASSERT(z == 1); // 检查是不是归一化
Eigen::Matrix xyz_uv_velocity;
xyz_uv_velocity
}
int endFrame();
};
id、起始帧 在每个帧中的属性 观测次数 是否是外点、是否被边缘化 估计深度是否收敛 结束帧
featurePerFrame类
class FeaturePerFrame
{
public:
FeaturePerFrame(const Eigen::Matrix &_point, double td)
{
point.x() = _point(0);
point.y() = _point(1);
point.z() = _point(2);
uv.x() = _point(3);
uv.y() = _point(4);
velocity.x() = _point(5);
velocity.y() = _point(6);
cur_td = td;
}
double cur_td;
Vector3d point;
Vector2d uv;
Vector2d velocity;
double z;
bool is_used;
double parallax;
MatrixXd A;
VectorXd b;
double dep_gradient;
};
每个特征点的归一化坐标、像素坐标、速度、id
6. 图解VINS特征点管理策略
 代码
for (auto &id_pts : image)
{
// 用特征点信息构造一个对象
FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
int feature_id = id_pts.first;
// 在已有的id中寻找是否是有相同的特征点
auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
{
return it.feature_id == feature_id;
});
// 这是一个新的特征点
if (it == feature.end())
{
// 在特征点管理器中，新创建一个特征点id，这里的frame_count就是该特征点在滑窗中的当前位置，作为这个特征点的起始位置
feature.push_back(FeaturePerId(feature_id, frame_count));
feature.back().feature_per_frame.push_back(f_per_fra);
}
// 如果这是一个已有的特征点，就在对应的“组织”下增加一个帧属性
else if (it->feature_id == feature_id)
{
it->feature_per_frame.push_back(f_per_fra);
last_track_num++; // 追踪到上一帧的特征点数目
}
}
遍历每个特征点，取出特征点id在feature中寻找id如果是新特征点，push到feature中如果是旧特征点，把featurePerFrame添加到featurePerId中
|